//
//  Cspace.m
//  SSE
//
//  Created by Matt Roman on 10/15/07.
//  Copyright 2007 __MyCompanyName__. All rights reserved.
//

#import "Cspace.h"


@implementation Cspace

/////////////////////////////////////////////////////////////////////////////////
// Initialization of Cspace with crate obstacles
// creates the initial rectangular bounding box
/////////////////////////////////////////////////////////////////////////////////
- (id)initCspaceWithCrate:(Crate *)obst ID:(int)num
{
	float gamma;
	Point3 *vertex;
	Point3f tempPt;
	
	[super init];
	color = 1;
	ObjID = num;
	spin = 0;
	spinLock = NO;
	center = [obst getPosition];
	
	radius = sqrt(([obst getLength] + CSPACE)*([obst getLength] + CSPACE) + 
			   ([obst getWidth] + CSPACE)*([obst getWidth] + CSPACE))/2.0;
	gamma = atan((([obst getWidth]+CSPACE)/2.0)/(([obst getLength]+CSPACE)/2.0));
	
	VertexArray = [[NSMutableArray alloc] init];
	
	tempPt.x = [obst getPosition].x - radius*cos(gamma + [obst getAlpha]);
	tempPt.y = [obst getPosition].y - radius*sin(gamma + [obst getAlpha]);
	tempPt.z = 0.0;
	
	[self setRadPoint:tempPt];
	vertex = [[Point3 alloc] initPoint3f:tempPt];
	[VertexArray addObject:vertex];
	[vertex release];
	
	tempPt.x = [obst getPosition].x - radius*cos(PI-gamma + [obst getAlpha]);
	tempPt.y = [obst getPosition].y - radius*sin(PI-gamma + [obst getAlpha]);
	tempPt.z = 0.0;
	
	vertex = [[Point3 alloc] initPoint3f:tempPt];
	[VertexArray addObject:vertex];
	[vertex release];
	
	tempPt.x = [obst getPosition].x + radius*cos(gamma + [obst getAlpha]);
	tempPt.y = [obst getPosition].y + radius*sin(gamma + [obst getAlpha]);
	tempPt.z = 0.0;
	
	vertex = [[Point3 alloc] initPoint3f:tempPt];
	[VertexArray addObject:vertex];
	[vertex release];
	
	tempPt.x = [obst getPosition].x + radius*cos(PI-gamma + [obst getAlpha]);
	tempPt.y = [obst getPosition].y + radius*sin(PI-gamma + [obst getAlpha]);
	tempPt.z = 0.0;
	
	vertex = [[Point3 alloc] initPoint3f:tempPt];
	[VertexArray addObject:vertex];
	[vertex release];
	
	return self;
}

//////////////////////////////////////////////////////////////////////////////////
// Draws a Cspace outline based on the number of verticies in the VertexArray
//////////////////////////////////////////////////////////////////////////////////
- (void)drawCspacePoint:(BOOL)pt RadLine:(BOOL)rad
{
	int i;
	Point3f *tempPt;
	
	glBegin(GL_LINE_LOOP);
	if(color)glColor4d(YELLOW);	
	else glColor4d(BLUE);
	
	for(i = 0; i < [VertexArray count]; i++)
	{
		tempPt = [[VertexArray objectAtIndex:i] getPtrPoint];
		glVertex3f(tempPt->x,tempPt->y,tempPt->z);
	}
	glEnd();
	if(pt){
		tempPt = [[VertexArray objectAtIndex:0] getPtrPoint];
		glPointSize(3.0);
		glBegin(GL_POINTS);
			glColor4f(WHITE);
			glVertex3f(tempPt->x,tempPt->y,0);
		glEnd();
		if([VertexArray count] > 21)
		{
			tempPt = [[VertexArray objectAtIndex:20] getPtrPoint];
			glBegin(GL_POINTS);
			glColor4f(RED);
			glVertex3f(tempPt->x,tempPt->y,0);
			glEnd();
		}
		if([VertexArray count] > 41)
		{
			tempPt = [[VertexArray objectAtIndex:40] getPtrPoint];
			glBegin(GL_POINTS);
			glColor4f(RED);
			glVertex3f(tempPt->x,tempPt->y,0);
			glEnd();
		}
		if([VertexArray count] > 61)
		{
			tempPt = [[VertexArray objectAtIndex:60] getPtrPoint];
			glBegin(GL_POINTS);
			glColor4f(RED);
			glVertex3f(tempPt->x,tempPt->y,0);
			glEnd();
		}
	}
	if(rad){
		glBegin(GL_LINES);
			glVertex3f(center.x,center.y,center.z);
			glVertex3f(radPoint.x,radPoint.y,radPoint.z);
		glEnd();
	}
	tempPt = nil;
}

- (void)setVertexArray:(NSMutableArray *)array
{
	[array retain];
	[VertexArray release];
	VertexArray = array;
}

- (NSMutableArray *)getVertexArray
{
	return VertexArray;
}
	
- (Point3f *)getCenter
{
	return &center;
}

- (void)setCenter:(Point3f)pt
{
	center = pt;
}

- (float)getRadius
{
	return radius;
}

- (void)setRadius:(float)r
{
	radius = r;
}

- (void)setRadPoint:(Point3f)pt
{
	radPoint = pt;
}

- (BOOL)isPointInside:(Point3f *)pt
{
	int i,iplus;
	double z;
	int side = 0;
	Point3f *p1, *p2;
	
	for(i=0; i<[VertexArray count]; i++)
	{
		if(i == [VertexArray count]-1) iplus = 0;
		else iplus = i+1;
		
		p1 = [[VertexArray objectAtIndex:i] getPtrPoint];
		p2 = [[VertexArray objectAtIndex:iplus] getPtrPoint];
		
		z = (pt->x - p1->x)*(p2->y - p1->y) - (pt->y - p1->y)*(p2->x - p1->x);
		if(z < 0) side++;
	}
	
	if(side == [VertexArray count]) return YES;
	else return NO;
}

- (int) getObjectNum
{
	return ObjID;
}

- (void) setObjectNum:(int)num
{
	ObjID = num;
}

- (void) setColor:(int)c;
{
	color = c;
}

- (int) spin
{
	return spin;
}

- (void) setSpin:(int)s
{
	spin = s;
}

- (bool) spinLock
{
	return spinLock;
}

- (void) setSpinLock:(bool)sl
{
	spinLock = sl;
}

- (void) dealloc
{	
	[VertexArray release];
	[super dealloc];
}
	
@end
